#include <cmath>
#include <string>
#include <cassert>

#include <QDebug>

#include "spiralv2.h"
#include "../../road/mapinfo.h"

#define GEOMETRYSTEPSIZE 0.1
#define DVARSTART 0.001
#define DVarSTEPSIZE 0.001
// 获取螺旋线点坐标二次多项式的系数
#define COORDINATEDFACTORA 0.001
#define COORDINATEDFACTORB 0.0000015708
#define COORDINATEDFACTORC 0.000000000524
#define COORDINATEDFACTORD 0.00000000000041

using namespace std;

SpiralV2::SpiralV2()
{

}

SpiralV2::~SpiralV2()
{

}

void SpiralV2::getGeometrySpiral(double dScaleFactor, double dVar, QVector<QPointF> &qvecGeometrySpiral, QVector<double> &qvecHdg, QVector<double> &qvecS)
{
    QPointF qpointf;
    qpointf = subFunctionSpiral(dScaleFactor, dVar);
    qvecGeometrySpiral.push_back(qpointf);
    qvecHdg.push_back(M_PI / 2 * pow(dVar, 2));
    qvecS.push_back(dScaleFactor * dVar);
}

void SpiralV2::convertHdgAndY(double dCurvatureStart, double dCurvatureEnd, QVector<QPointF> &qvecGeometrySpiral, QVector<double> &qvecHdg)
{
    if((MapInfo::isZero(dCurvatureStart)) && (dCurvatureEnd > 0))
    {
        assert("convertHdgAndY other case");
    }
    else if((MapInfo::isZero(dCurvatureStart)) && (dCurvatureEnd < 0))
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = -qvecHdg[i];
            qvecGeometrySpiral[i].setY(-qvecGeometrySpiral[i].ry());
        }
    }
    else if((dCurvatureStart > 0) && (MapInfo::isZero (dCurvatureEnd)))
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = -qvecHdg[i] - M_PI;
            qvecGeometrySpiral[i].setY(-qvecGeometrySpiral[i].ry());
        }
    }
    else
    {
        for(int i = 0; i < qvecHdg.size(); ++i)
        {
            qvecHdg[i] = qvecHdg[i] - M_PI;
        }
    }
}

void SpiralV2::convertIndexStartAndEnd(bool bStart, double dLength, QVector<QPointF> &qvecGeometrySpiral, QVector<double> &qvecHdg, QVector<double> &qvecS)
{
    if(!bStart)
    {
        int iNum = qvecS.size();
        double dHdgTmp, dSTmp;

        for(int i = 0; i < iNum / 2; ++i)
        {
            QPointF qpointf = qvecGeometrySpiral[i];
            qvecGeometrySpiral[i] = qvecGeometrySpiral[iNum - i - 1];
            qvecGeometrySpiral[iNum - i - 1] = qpointf;
            dHdgTmp = qvecHdg[i];
            qvecHdg[i] = qvecHdg[iNum - i - 1];
            qvecHdg[iNum - i - 1] = dHdgTmp;
            dSTmp = dLength - qvecS[i];
            qvecS[i] = dLength - qvecS[iNum - i - 1];
            qvecS[iNum - i - 1] = dSTmp;
        }
    }
}

void SpiralV2::rotateCoordinate(double dSpinHdg, QVector<QPointF> &qvecGeometrySpiral)
{
    double dCurrentXCoordinate, dCurrentYCoordinate, dNewXCoordinate, dNewYCoordinate, dAlpha, dBeta, dDistance;

    for(int i = 0; i < qvecGeometrySpiral.size(); ++i)
    {
        dCurrentXCoordinate = qvecGeometrySpiral[i].rx();
        dCurrentYCoordinate = qvecGeometrySpiral[i].ry();
        dDistance = sqrt(pow(dCurrentXCoordinate, 2) + pow(dCurrentYCoordinate, 2));

        if(MapInfo::isZero(dCurrentXCoordinate))
        {
            if(dCurrentYCoordinate > 0)
            {
                dAlpha = M_PI / 2;
            }
            else if(dCurrentYCoordinate < 0)
            {
                dAlpha = -M_PI / 2;
            }
            else
            {
                dAlpha = 0;
            }
        }
        else
        {
            dAlpha = atan(dCurrentYCoordinate / dCurrentXCoordinate);
        }

        dBeta = dAlpha + dSpinHdg;
        dNewXCoordinate = dDistance * cos(dBeta);
        dNewYCoordinate = dDistance * sin(dBeta);
        qvecGeometrySpiral[i].setX(dNewXCoordinate);
        qvecGeometrySpiral[i].setY(dNewYCoordinate);
    }
}

void SpiralV2::testSpiral(QVector<QPointF> &qvecGeometrySpiral, QVector<double> &qvecHdg, QVector<double> &qvecS)
{
    string strX, strY, strHdg, strS;

    for ( int i = 0; i < qvecGeometrySpiral.size(); ++i)
    {
        //输出测试
        strX = to_string(qvecGeometrySpiral[i].rx());
        strY = to_string(qvecGeometrySpiral[i].ry());
        strHdg = to_string(qvecHdg[i]);
        strS = to_string(qvecS[i]);
        qDebug() << strX.c_str() << " " << strY.c_str() << " " << strHdg.c_str() << " " << strS.c_str();
    }
}

//螺旋线数据获取
PointHdg SpiralV2::geometrySpiralData(double dXCoordinate, double dYCoordinate, double dHdg, double dLength, double dCurvatureStart, double dCurvatureEnd)
{
    QTextStream out(stdout);
    PointHdg geometrySpiral;

    if((dCurvatureStart * dCurvatureEnd) < 0)
    {
        qDebug() << "There should be no such road";
        return geometrySpiral;
    }

    QVector<QPointF> qvecGeometrySpiral;
    QVector<double> qvecHdg;
    QVector<double> qvecS;

    // dScaleFactor is formula variable A
    double dScaleFactor = sqrt(M_PI * dLength / fabs(dCurvatureStart - dCurvatureEnd));
    double dStartVariable = fabs(dScaleFactor * dCurvatureStart / M_PI);
    double dEndVariable = fabs(dScaleFactor * dCurvatureEnd / M_PI);
    double dMinVar, dMaxVar;
    bool bStart = true;

    if(dStartVariable <= dEndVariable)
    {
        dMinVar = dStartVariable;
        dMaxVar = dEndVariable;
    }
    else
    {
        dMinVar = dEndVariable;
        dMaxVar = dStartVariable;
        bStart = false;
    }

    for(double dVar = dMinVar; dVar < dMaxVar; dVar += GEOMETRYSTEPSIZE)
    {
        getGeometrySpiral(dScaleFactor, dVar, qvecGeometrySpiral, qvecHdg, qvecS);
    }

    getGeometrySpiral(dScaleFactor, dMaxVar, qvecGeometrySpiral, qvecHdg, qvecS);

    // 转换Hdg和Y
    convertHdgAndY(dCurvatureStart, dCurvatureEnd, qvecGeometrySpiral, qvecHdg);

    double dSpinHdg = bStart ? dHdg - qvecHdg[0] : dHdg - qvecHdg[qvecHdg.size() - 1];

    // 旋转坐标系
    rotateCoordinate(dSpinHdg, qvecGeometrySpiral);

    double dPanXCoordinate, dPanYCoordinate;

    if(bStart)
    {
        dPanXCoordinate = dXCoordinate - qvecGeometrySpiral[0].rx();
        dPanYCoordinate = dYCoordinate - qvecGeometrySpiral[0].ry();
    }
    else
    {
        dPanXCoordinate = dXCoordinate - qvecGeometrySpiral[qvecGeometrySpiral.size() - 1].rx();
        dPanYCoordinate = dYCoordinate - qvecGeometrySpiral[qvecGeometrySpiral.size() - 1].ry();
    }

    for(int i = 0; i < qvecGeometrySpiral.size(); ++i)
    {
        qvecGeometrySpiral[i].setX(qvecGeometrySpiral[i].rx() + dPanXCoordinate);
        qvecGeometrySpiral[i].setY(qvecGeometrySpiral[i].ry() + dPanYCoordinate);
    }

    for(int i = 0; i < qvecHdg.size(); ++i)
    {
        qvecHdg[i] = qvecHdg[i] + dSpinHdg;
    }

    // 调整螺旋线的起止点
    convertIndexStartAndEnd(bStart, dLength, qvecGeometrySpiral, qvecHdg, qvecS);

    geometrySpiral.qvecPointData = qvecGeometrySpiral;
    geometrySpiral.qvecHdgData = qvecHdg;
    geometrySpiral.qvecSData = qvecS;

    testSpiral(qvecGeometrySpiral, qvecHdg, qvecS);

    return geometrySpiral;
}

//螺旋线子函数
QPointF SpiralV2::subFunctionSpiral(double dScaleFactor, double dVarValue)
{
    QPointF point;
    double dCurrentXCoordinate = 0;
    double dCurrentYCoordinate = 0;
    double dVar;

    for(dVar = DVARSTART; dVar <= dVarValue; dVar += DVarSTEPSIZE)
    {
        double dTmp = M_PI * pow(dVar, 2) / 2;
        double dCosTmp = cos(dTmp);
        double dSinTmp = sin(dTmp);
        dCurrentXCoordinate = dCurrentXCoordinate + COORDINATEDFACTORA * dCosTmp - COORDINATEDFACTORB * dVar * dSinTmp - COORDINATEDFACTORC * (dSinTmp + M_PI * pow(dVar, 2) * dCosTmp) - COORDINATEDFACTORD * dVar * (3 * dCosTmp - M_PI * pow(dVar, 2) * dSinTmp);
        dCurrentYCoordinate = dCurrentYCoordinate + COORDINATEDFACTORA * dSinTmp + COORDINATEDFACTORB * dVar * dCosTmp + COORDINATEDFACTORC * (dCosTmp - M_PI * pow(dVar, 2) * dSinTmp) - COORDINATEDFACTORD * dVar * (3 * dSinTmp + M_PI * pow(dVar, 2) * dCosTmp);
    }

    point.setX(dScaleFactor * dCurrentXCoordinate);
    point.setY(dScaleFactor * dCurrentYCoordinate);
    return point;
}

